#include "includes.h"
#include <string.h> 
#include <stdio.h>  
#include <stdbool.h>
#include <IOdevice.h>
#include "xrj_protocol.h"

#include "Motor.h"

//--------电机控制-------------------------------------------------------------------------------------------


#define	GPIO_Pin_Motor1_P		GPIO_Pin_9	// 右电机
#define	GPIO_Pin_Motor1_N		GPIO_Pin_8

#define	GPIO_Pin_Motor2_P		GPIO_Pin_4  // 左电机
#define	GPIO_Pin_Motor2_N		GPIO_Pin_3


#define	P_Out_Motor1_P	 		PBout(9)
#define	P_Out_Motor1_N	 		PBout(8)

#define	P_Out_Motor2_P	 		PBout(4)
#define	P_Out_Motor2_N	 		PBout(3)



enum {
	Motor1_ID = 0,
	Motor2_ID =	1,	
};

typedef struct _st_Motor_Manage{
	UINT32 MotorCmd;	//==收到命令
	
	UINT32 ResetCmd;	//==电机卡住后，反向复位命令
	UINT32 ResetCnt;	//==复位次数
	
	UINT32 RunCnt;	
	UINT32 RunStatus;
	
	UINT32 StopCnt;
	UINT32 StopStatus;
}st_Motor_Manage;
enum{
	Motor_Stop = 0,
	Motor_Foreward = 1,
	Motor_Reversal = 2,
};
enum{
	MOTOR_STOP_DIS   = 0,
	MOTOR_STOP_EN    = 1,
	MOTOR_STOP_DOING = 2,
};

#define	D_STOP_DELAY 	30
#define	D_RUN_DELAY 	500


#define STEP_RUN		(0)
#define STEP_STOP2		(STEP_RUN+D_RUN_DELAY)
#define STEP_END		(STEP_STOP2+D_STOP_DELAY)



st_Motor_Manage Motor_Manage[2] = 
{
	{Motor_Stop, Motor_Stop, 0, 0, Motor_Stop, 0, MOTOR_STOP_DIS},
	{Motor_Stop, Motor_Stop, 0, 0, Motor_Stop, 0, MOTOR_STOP_DIS},
};


void MOTOR_Init(void)
{				
	GPIO_InitTypeDef  GPIO_InitStructure; 
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE); //原来的初始化代码没有打开时钟 
	
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable , ENABLE);
	
	GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_Motor1_P|GPIO_Pin_Motor1_N|GPIO_Pin_Motor2_P|GPIO_Pin_Motor2_N; 
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;     
	GPIO_Init(GPIOB, &GPIO_InitStructure); 	
		
	P_Out_Motor1_P = 0;
	P_Out_Motor1_N = 0;
	
	P_Out_Motor2_P = 0;
	P_Out_Motor2_N = 0;
	
}


void MOTOR_Foreward(UINT32 id)
{
	if (id == Motor1_ID)
	{
		P_Out_Motor1_N = 0;
		P_Out_Motor1_P = 1;	
	}	
	else if (id == Motor2_ID)
	{
		P_Out_Motor2_N = 0;
		P_Out_Motor2_P = 1;		
	}
	else
	{
		return;
	}	
	Motor_Manage[id].RunStatus = Motor_Foreward;
	Motor_Manage[id].StopStatus = MOTOR_STOP_EN;
}	


void MOTOR_Reversal(UINT32 id)
{
	if (id == Motor1_ID)
	{
		P_Out_Motor1_P = 0;
		P_Out_Motor1_N = 1;
	}	
	else if (id == Motor2_ID)
	{	
		P_Out_Motor2_P = 0;
		P_Out_Motor2_N = 1;
	}
	else
	{
		return;
	}	
	Motor_Manage[id].RunStatus = Motor_Reversal;
	Motor_Manage[id].StopStatus = MOTOR_STOP_EN;
}	

void MOTOR_Stop(UINT32 id)
{
	if (id == Motor1_ID)
	{
		P_Out_Motor1_N = 0;
		P_Out_Motor1_P = 0;
	}	
	else if (id == Motor2_ID)
	{	
		P_Out_Motor2_N = 0;
		P_Out_Motor2_P = 0;
	}
	else
	{
		return;
	}	
	Motor_Manage[id].RunStatus = Motor_Stop;
}	
	
void MOTOR_StopInit(UINT32 id)
{
	if (Motor_Manage[id].StopStatus == MOTOR_STOP_EN)
	{
		Motor_Manage[id].StopStatus = MOTOR_STOP_DOING;
		Motor_Manage[id].StopCnt = 0;
		MOTOR_Stop(id);
	}	
}	

void Motor_Proc(void)
{
	UINT32 PLATE_Setting[2]; 
	UINT32 PLATE_Report[2]; 
	
	UINT32 i;
	XRJ_mutex_lock(&Device_Value.Lock);	
	PLATE_Setting[0] = Device_Value.PLATE_1_Setting; 
	PLATE_Setting[1] = Device_Value.PLATE_2_Setting; 
	Device_Value.PLATE_1_Setting = IOT__STATUS_CODES__NONE;
	Device_Value.PLATE_2_Setting = IOT__STATUS_CODES__NONE;
	PLATE_Report[0] = Device_Value.PLATE_1_Report; 
	PLATE_Report[1] = Device_Value.PLATE_2_Report; 	
	XRJ_mutex_unlock(&Device_Value.Lock);
	for (i = 1; i < 2; i++)
	{
	
		if (PLATE_Setting[i] == IOT__STATUS_CODES__ON)
		{
			if (Motor_Manage[i].MotorCmd != Motor_Foreward)
			{	
				Motor_Manage[i].MotorCmd = Motor_Foreward;
				Motor_Manage[i].RunCnt = 0;	
				MOTOR_Foreward(Motor1_ID);
				MOTOR_StopInit(i);
			}	
		}	

		if (PLATE_Setting[i] == IOT__STATUS_CODES__OFF)
		{
			if (Motor_Manage[i].MotorCmd != Motor_Reversal)
			{	
				Motor_Manage[i].MotorCmd = Motor_Reversal;
				Motor_Manage[i].RunCnt = 0;	
				MOTOR_Stop(Motor1_ID);
				MOTOR_StopInit(i);
			}	
		}		
	}	
	
	for (i = 1; i < 2; i++)
	{
		if (Motor_Manage[i].StopStatus == MOTOR_STOP_DOING)
		{
			
			Motor_Manage[i].StopCnt++;
			if (Motor_Manage[i].StopCnt >= D_STOP_DELAY)		
			{
				Motor_Manage[i].StopStatus = MOTOR_STOP_DIS;
			}			
		}	
		else
		{
			if ((Motor_Manage[i].MotorCmd == Motor_Foreward) ||  (Motor_Manage[i].MotorCmd == Motor_Reversal))
			{
				if (Motor_Manage[i].RunCnt == STEP_RUN)
				{
					if (Motor_Manage[i].MotorCmd == Motor_Foreward)	
						MOTOR_Foreward(i);					
					else
						MOTOR_Reversal(i);			
				}	
				else if (Motor_Manage[i].RunCnt >= STEP_STOP2)
				{
					MOTOR_StopInit(i);
					Motor_Manage[i].MotorCmd = Motor_Stop;
				}	
				Motor_Manage[i].RunCnt++;	
			}	
		}
			
	}	

	for (i = 1; i < 2; i++)
	{
		if (Motor_Manage[i].MotorCmd == Motor_Foreward)
		{	
			if (PLATE_Report[i] == IOT__STATUS_CODES__ON)
			{
				MOTOR_StopInit(i);
				Motor_Manage[i].MotorCmd = Motor_Stop;				
			}	
		}	
		
		if (Motor_Manage[i].MotorCmd == Motor_Reversal)
		{	
			if (PLATE_Report[i] == IOT__STATUS_CODES__OFF)
			{
				MOTOR_StopInit(i);
				Motor_Manage[i].MotorCmd = Motor_Stop;				
			}	
		}	
	}

		
}



#define	GPIO_Pin_FAN		GPIO_Pin_8


#define	P_Out_FAN	 		PAout(8)




void FAN_Init(void)
{				
	GPIO_InitTypeDef  GPIO_InitStructure; 
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO, ENABLE); //原来的初始化代码没有打开时钟 
	

	
	GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_FAN; 
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;     
	GPIO_Init(GPIOA, &GPIO_InitStructure); 	
	P_Out_FAN = 0;	
}

void FAN_Open(void)
{
	P_Out_FAN = 1;
}	

void FAN_Close(void)
{
	P_Out_FAN = 0;
}

void FAN_PROC(void)
{
	Iot__StatusCodes	Fan_Set;
	XRJ_mutex_lock(&Device_Value.Lock);	
	Fan_Set = Device_Value.FAN_Setting; 
 	Device_Value.FAN_Setting = IOT__STATUS_CODES__NONE;
	XRJ_mutex_unlock(&Device_Value.Lock);

	if (Fan_Set == IOT__STATUS_CODES__ON)
	{
		FAN_Open();
	}
	else if (Fan_Set == IOT__STATUS_CODES__OFF)
	{
		FAN_Close();	
	}	
}	






#define	GPIO_Pin_LOCK_1		GPIO_Pin_0
#define	GPIO_Pin_LOCK_2		GPIO_Pin_2

#define	P_Out_LOCK_1	 		PCout(0)
#define	P_Out_LOCK_2	 		PCout(2)
#define	P_Out_SW1	 	 		PCout(4)

void LOCK_Init(void)
{				
	GPIO_InitTypeDef  GPIO_InitStructure; 
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE); //原来的初始化代码没有打开时钟 
	

	
	GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_LOCK_1|GPIO_Pin_LOCK_2; 
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;     
	GPIO_Init(GPIOC, &GPIO_InitStructure); 	
	//默认开
//	P_Out_LOCK_1 = 1;	
//	P_Out_LOCK_2 = 1;
}
void LOCK_Open(UINT32 index)
{
	
	if (index == IOT__UNIT_INDEX__LOCK_1)
	{
		printf("lock1 open\n");
		P_Out_LOCK_1 = 1;
	}
	else if(index == IOT__UNIT_INDEX__LOCK_2)
	{
		printf("lock2 open\n");
		P_Out_LOCK_2 = 1;
	}	
	else if (index == IOT__UNIT_INDEX__LIGHT_INSIDE_1)
	{
		P_Out_SW1 = 1;
	}
}


void LOCK_Close(UINT32 index)
{
	
	if (index == IOT__UNIT_INDEX__LOCK_1)
	{
		P_Out_LOCK_1 = 0;
		printf("lock1 close\n");
	}
	else if(index == IOT__UNIT_INDEX__LOCK_2)
	{
		P_Out_LOCK_2 = 0;
		printf("lock2 close\n");
	}	
	else if (index == IOT__UNIT_INDEX__LIGHT_INSIDE_1)
	{
		P_Out_SW1 = 0;
	}
}

UINT32 Lock_Mode1 = 0;
UINT32 Lock_Cnt1 = 0;
UINT32 Lock_Mode2 = 0;
UINT32 Lock_Cnt2 = 0;
UINT32 Lock_Mode3 = 0;
UINT32 Lock_Cnt3 = 0;

#define	D_LOCK_TIME		50

void LOCK_PROC(void)
{
	
	Iot__StatusCodes	LOCK_Set_1;
	Iot__StatusCodes	LOCK_Set_2;
	Iot__StatusCodes	LOCK_Set_3;
	
	XRJ_mutex_lock(&Device_Value.Lock);	
	LOCK_Set_1 = Device_Value.LOCK_1_Setting;
	LOCK_Set_2 = Device_Value.LOCK_2_Setting;	
	LOCK_Set_3 = Device_Value.LIGHT_INSIDE_1_Setting;
 	Device_Value.LOCK_1_Setting = IOT__STATUS_CODES__NONE;
	Device_Value.LOCK_2_Setting = IOT__STATUS_CODES__NONE;
	Device_Value.LIGHT_INSIDE_1_Setting = IOT__STATUS_CODES__NONE;
	XRJ_mutex_unlock(&Device_Value.Lock);

	if (LOCK_Set_1 == IOT__STATUS_CODES__ON)
	{
		if (Lock_Mode1 == 0)
		{
			Lock_Mode1 = 1;
			Lock_Cnt1 = 0;
			LOCK_Open(IOT__UNIT_INDEX__LOCK_1);
		}		
	}
	if (Lock_Mode1 != 0)
	{
		Lock_Cnt1++;
		if (Lock_Cnt1 > D_LOCK_TIME)
		{
			Lock_Mode1 = 0;
			LOCK_Close(IOT__UNIT_INDEX__LOCK_1);	
		}		
	}
	
	
	
	if (LOCK_Set_2 == IOT__STATUS_CODES__ON)
	{
		if (Lock_Mode2 == 0)
		{
			Lock_Mode2 = 1;
			Lock_Cnt2 = 0;
			LOCK_Open(IOT__UNIT_INDEX__LOCK_2);
		}		
	}
	if (Lock_Mode2 != 0)
	{
		Lock_Cnt2++;
		if (Lock_Cnt2 > D_LOCK_TIME)
		{
			Lock_Mode2 = 0;
			LOCK_Close(IOT__UNIT_INDEX__LOCK_2);	
		}		
	}
	
	if (LOCK_Set_3 == IOT__STATUS_CODES__ON)
	{
		if (Lock_Mode3 == 0)
		{
			Lock_Mode3 = 1;
			Lock_Cnt3 = 0;
			LOCK_Open(IOT__UNIT_INDEX__LIGHT_INSIDE_1);
		}		
	}
	if (Lock_Mode3 != 0)
	{
		Lock_Cnt3++;
		if (Lock_Cnt3 > D_LOCK_TIME)
		{
			Lock_Mode3 = 0;
			LOCK_Close(IOT__UNIT_INDEX__LIGHT_INSIDE_1);	
		}		
	}


}	











